% PathSim2.m
%
% Simulates the path of a vehicle at the SFE AVC site; 56 meter by 72 meter path with 5m radius turns
% at each corner.  Simulates compass and gyro noise and gyro drift.
%
% Then prototypes filter to estimate position and creates plots to compare performance of filter with
% actual path, gyro-only, and compass-only estimates.
%
% Sets:
% S -- system state, perfect theoretical, no noise, etc.
% M -- measurements of heading and heading rate corrupted by noise

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Functions
%

% turn simulates turning a corner 
%
% x - initial x
% y - initial y
% s - speed
% h - heading
% r - radius of turn
% drot - direction of rotation: 1 is CW, -1 is CCW
function P=turn(x, y, s, h, r, drot)
	dt=0.01;					% time step, seconds
	c=pi*r/2;					% circumference for quarter circle
	T=[0:dt:c/s]';				% measurement times
	w=s/r;						% hdg rate of change rad/sec	
	W=w*ones(length(T),1);
	H0 = h*pi/180*ones(length(T),1);
	H = H0 + drot*w*T;			% integrate heading
	x += r*cos(h*pi/180);		% offset by radius
	y += -r*sin(h*pi/180);		% offset by radius
	X = -drot*r*cos(H)+x;
	Y = drot*r*sin(H)+y;
	V = s*ones(length(T),1);	% speed
	H *= 180/pi;
	W *= 180/pi;
	P=[X, Y, V, H, W];
end

% straight simulates driving straight
%
% x - initial x
% y - initial y
% s - speed
% h - heading
% d - distance travelled
function P=straight(x,y,s,h,d)
	T=[0:0.01:d/s]';
	X0 = x*ones(length(T),1);
	Y0 = y*ones(length(T),1);
	H=h*ones(length(T),1);
	X=X0 + T*s*sin(h*pi/180);
	Y=Y0 + T*s*cos(h*pi/180);
	V = s*ones(length(T),1);	% speed
	W = zeros(length(H),1);
	P=[X, Y, V, H, W];
end

% measurements corrupts the state matrix with noise
%
% S    = system state (theoeretically perfect)
% hvar = variance of heading noise
% wvar = variance of heading rate noise
% zvar = variance of system noise
% drift = gyro drift in rad/sec
function M=measurements(S, hvar, wvar, svar, zvar, drift)
	% can do process noise here
	% and gyro drift here
	% Theoretical
	[ m n ]=size(S);
	Vz = S(:,3);							% speed with system noise
	Hz = S(:,4);							% heading with system noise
	Wz = S(:,5);							% heading rate with system noise
	% Sensor measurements
	%
	%%%%%%%%%%%%%%% TODO %%%%%%%%%%%%%%%%%
	% Add bias terms
	% Simulate compass mis-calibration
	%
	Hn = Hz + hvar*stdnormal_rnd(m,1);				% Simulated compass sensor noise
	Wn = Wz + wvar*stdnormal_rnd(m,1) + drift*ones(m,1);		% Simulated gyro sensor noise + drift
	Vn = Vz + svar*stdnormal_rnd(m,1);				% Simulated speed sensor noise
	% Setup measurements matrix
	M = [ Hn, Wn, Vn ];
end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Setup main parameters
r=5;						% turn radius
s=10;						% speed
lx=56;						% distance, x
ly=72;						% distance, y
d=10000;					% drift, deg/hr
hvar=1;						% heading sensor noise
wvar=2;						% gyro sensor noise
svar=0.5;					% encoder sensor noise
zvar=0;						% system noise
hdg=[-90, 0, 90, 180, 270];	% headings

dt=0.01;

d = d/3600;					% convert to deg/sec

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Setup System State matrix
%
% SYS in form of [X, Y, V, H, W];
%
SYS=[
	straight(lx/2, 0, s, hdg(1), lx/2-r); ...
	turn(r, 0, s, hdg(1), r, 1); ...
	straight(0, r, s, hdg(2), ly-r*2); ...
	turn(0, ly-r, s, hdg(2), r, 1); ...
	straight(r, ly, s, hdg(3), lx-r*2); ...
	turn(lx-r, ly, s, hdg(3), r, 1); ...
	straight(lx, ly-r, s, hdg(4), ly-r*2); ...
	turn(lx, r, s, hdg(4), r, 1); ...
	straight(lx-r, 0, s, hdg(5), lx/2-r); ...
	];	

%SYS=[
%	straight(lx, 0, s, hdg(1), 500)
%	];
	
% Determine time vector for plotting
T=dt*[1:length(SYS(:,3))]';
% Obtain drift- and noise-corrupted measurements
%
% Measurements in form of [ Hn, Wn, Vn ]
%
M=measurements(SYS, hvar, wvar, svar, zvar, d);
%
% Kalman Filter Setup
%
% x=[hdg; bias] u=[hdg-rate]
%
A = [1 -dt; 0 1];						% Transition matrix
B = [ dt; 0 ];							% External input transition
u = 0;									% External motion
H = [ 1 0 ];							% Maps measurement to state vector
P = [ 100 0; 0 100 ];					% Covariance of estimate; how certain is our estimate?
Q = [ .025 0; 0 .025 ];					% System noise (bumps in road, etc)
R = [ 0.5 ];							% Measurement noise
I = eye(2);								% Identity matrix
x = [ hdg(1); 0 ];						% State vector consisting of heading and heading-rate
% Initialize x w/ initial state
Hk = zeros(length(T),1);				% kalman filter estimate
Wk = zeros(length(T),1);				% kalman filter estimate
Bk = zeros(length(T),1);				% kalman filter estimate
for i=1:length(T)
	%
	% Predict
	%
	u = M(i,2);							% gyro becomes the input, indirect-Kalman style
	%
	% In this step we "move" our state estimate according to the equation:
	x = A*x + B*u;						% Eq 1.9
	% We also have to "move" our uncertainty and add noise. Whenever we move,
	% we lose certainty because of system noise
	P = A*P*A' + Q;						% Eq 1.10
	%
	% Measurement aka Correct
	%
	z = [ M(i,1) ];						% put measurement into z matrix
	%
	% First, we have to figure out the Kalman Gain which is basically how much we
	% trust the sensor measurement versus our prediction.
	K = P*H'*inv(H*P*H' + R);			% Eq 1.11
	% Then we determine the discrepancy between prediction and measurement with
	% the "Innovation" or Residual: z-H*x, multiply that by the Kalman gain to
	% correct the estimate towards the prediction a little at a time. 
	x = x + K*(z-H*x);					% Eq 1.12
	% We also have to adjust the certainty. With a new measurement, the estimate
	% certainty always increases.
	P = (I-K*H)*P;						% Eq 1.13
	%
	% Populate the heading and heading rate matrices for plotting
	Hk(i) = x(1);
	Bk(i) = x(2);
	Wk(i) = u - x(2);
end
%
% Estimates based on heading (compass) and heading rate (gyro), and Kalman Filter
%
Xw = zeros(length(T),1);	% gyro-based position estimate
Xw(1) = lx/2;
Yw = zeros(length(T),1);	% gyro-based position estimate
Hw = zeros(length(T),1);	% gyro-based heading estimate
Hw(1) = [ hdg(1) ];
Xh = zeros(length(T),1);	% compass-based position estimate
Xh(1) = lx/2;
Yh = zeros(length(T),1);	% compass-based position estimate
Xk = zeros(length(T),1);	% compass-based position estimate
Xk = lx/2;
Yk = zeros(length(T),1);	% compass-based position estimate
for i=1:length(T)-1
	Hw(i+1) = Hw(i) + dt*M(i,2);
	Xw(i+1) = Xw(i) + dt*M(i,3)*sin(Hw(i)*pi/180);
	Yw(i+1) = Yw(i) + dt*M(i,3)*cos(Hw(i)*pi/180);
	Xh(i+1) = Xh(i) + dt*M(i,3)*sin(M(i,1)*pi/180);
	Yh(i+1) = Yh(i) + dt*M(i,3)*cos(M(i,1)*pi/180);
	Xk(i+1) = Xk(i) + dt*M(i,3)*sin(Hk(i)*pi/180);
	Yk(i+1) = Yk(i) + dt*M(i,3)*cos(Hk(i)*pi/180);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Plot the results
%
% Position
%
close all;
lm=max([lx, ly]);			% for scaling the plot
figure;
plot(SYS(:,1), SYS(:,2), '-', Xw, Yw, '-', Xh, Yh, '-', Xk, Yk, '-');
legend("Theoretical", "Est, Gyro", "Est, Comp", "Est, Kalman", "location", "southeast");
xlabel("X (m)", "fontsize", 14);
ylabel("Y (m)", "fontsize", 14);
title("Position", "fontsize", 20);
axis([-5, lm+5, -5, lm+5]);
axis square;
grid on;
%
% Speed
%
%figure;
%plot(T, SYS(:,3), '-', T, M(:,3), '.');
%xlabel("time (s)", "fontsize", 14);
%ylabel("speed (m/s)", "fontsize", 14);
%title("Speed", "fontsize", 20);
%axis([0, T(length(T)), 0, s+5]);
%grid on;
%
% Heading
%
figure;
plot(T, SYS(:,4), '-', T, M(:,1), '.', T, Hw, '-', T, Hk, '-');
legend("Theoretical", "Measured", "Gyro Est", "Kalman est", "location", "southeast");
xlabel("time (s)", "fontsize", 14);
ylabel("heading (deg)", "fontsize", 14);
title("Heading", "fontsize", 20);
grid on;
%
% Heading Rate
%
figure;
plot(T, SYS(:,5),'-', T, M(:,2), '.', T, Wk, '-');
legend("Theoretical", "Measured", "Kalman est", "location", "southeast");
xlabel("time (s)", "fontsize", 14);
ylabel("heading rate (deg/s)", "fontsize", 14);
title("Heading rate", "fontsize", 20);
grid on;
